#include "bsp_motor.h"
#include "bsp.h"



// Ignore PWM dead band  忽略PWM信号死区
static int16_t Motor_Ignore_Dead_Zone(int16_t pulse)
{
    if (pulse > 0) return pulse + MOTOR_IGNORE_PULSE;
    if (pulse < 0) return pulse - MOTOR_IGNORE_PULSE;
    return 0;
}

// The PWM port of the motor is initialized  电机PWM口初始化
/**
 * @brief 初始化电机控制所需的定时器
 * 
 * 本函数启动了两个定时器实例（htim1和htim8）的PWM输出，
 * 用于驱动电机。其中，htim1负责控制一组电机，htim8负责
 * 控制另一组电机。通过启动定时器的PWM输出，可以实现对
 * 电机速度和方向的精确控制。
 * 
 * 注意：这里使用了TIM_CHANNEL_1至TIM_CHANNEL_4表示定时器的
 * 四个通道，分别对应于电机控制中的四个PWM信号。
 */
void Motor_Init(void)
{
    /* 启动htim1的第1、2、3、4通道的PWM输出，用于控制一组电机 */
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
    
    /* 启动htim8的第1、2、3、4通道的PWM输出，用于控制另一组电机 */
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4);
}

// All motors stopped  所有电机停止
void Motor_Stop(uint8_t brake)
{
    if (brake != 0)
    {
    	brake = 1;
    }

    PWM_M1_A = 0;
    PWM_M1_B = 0;
    PWM_M2_A = 0;
    PWM_M2_B = 0;
    PWM_M3_A = 0;
    PWM_M3_B = 0;
    PWM_M4_A = 0;
    PWM_M4_B = 0;
}

// 设置电机速度，speed:±（3600-MOTOR_IGNORE_PULSE）, 0为停止
// Set motor speed, speed:± (3600-MOTOR_IGNORE_PULSE), 0 indicates stop
/**
 * 设置电机速度，速度范围：±(3600-MOTOR_IGNORE_PULSE)，0表示停止
 * @param id 电机标识符，用于指定要设置速度的电机
 * @param speed 期望的电机速度，正数表示正转，负数表示反转
 */
void Motor_Set_Pwm(uint8_t id, int16_t speed)
{
    int16_t pulse = Motor_Ignore_Dead_Zone(speed); // 调整速度值，忽略死区

    if (pulse >= MOTOR_MAX_PULSE)
        pulse = MOTOR_MAX_PULSE;
    if (pulse <= -MOTOR_MAX_PULSE)
        pulse = -MOTOR_MAX_PULSE;

    switch (id)
    {
    case MOTOR_ID_M1:
    {
        pulse = -pulse;         // 为M1电机反转
        if (pulse >= 0)
        {
            PWM_M1_A = pulse;   // 正转时设置PWM
            PWM_M1_B = 0;
        }
        else
        {
            PWM_M1_A = 0;       // 反转时设置PWM
            PWM_M1_B = -pulse;
        }
        break;
    }
    case MOTOR_ID_M2:
    {
        pulse = -pulse;         // 为M2电机反转
        if (pulse >= 0)
        {
            PWM_M2_A = pulse;   // 正转时设置PWM
            PWM_M2_B = 0;
        }
        else
        {
            PWM_M2_A = 0;       // 反转时设置PWM
            PWM_M2_B = -pulse;
        }
        break;
    }

    case MOTOR_ID_M3:
    {
        if (pulse >= 0)
        {
            PWM_M3_A = pulse;   // 正转时设置PWM
            PWM_M3_B = 0;
        }
        else
        {
            PWM_M3_A = 0;       // 反转时设置PWM
            PWM_M3_B = -pulse;
        }
        break;
    }
    case MOTOR_ID_M4:
    {
        if (pulse >= 0)
        {
            PWM_M4_A = pulse;   // 正转时设置PWM
            PWM_M4_B = 0;
        }
        else
        {
            PWM_M4_A = 0;       // 反转时设置PWM
            PWM_M4_B = -pulse;
        }
        break;
    }

    default:
        break; // 如果传入的电机ID无效，则不执行任何操作
    }
}

